#!/usr/bin/env python
import os
import rospy
import actionlib
import json
import requests
import time
import signal
import sys
import threading
from socket_utils import SocketServer
from semantic_nav.msg import NavStrAction, NavStrGoal


def signal_handler(sig, frame):
    print("接收到信号，程序即将退出...")
    sys.exit(0)


signal.signal(signal.SIGINT, signal_handler)  # 注册 Ctrl+C 信号处理函数


def return_to_ui(ctx, response):
    headers = {
        'Content-Type': 'application/json',
        'Authorization': ctx.get('auth'),
    }
    # print(headers)
    data = {
        'session': ctx.get('session'),
        'member': int(ctx.get('member')),  # 注意这里是int不是str
        'text': response,
    }
    # print(data)
    resp = requests.post(
        url='http://127.0.0.1:8080/api/dialog/',   # Meadows的接口，IP和端口可能会变
        headers=headers,
        data=json.dumps(data)
    )
    
    # print(resp.text)
    if resp.status_code == 201:
        print('Success')
    else:
        print('Failed')


class SemanticNavClient:
    def __init__(self) -> None:
        rospy.init_node('semantic_nav_client')
        self.client = actionlib.SimpleActionClient('semantic_nav', NavStrAction)
        self.client.wait_for_server()
    
    def active_callback(self):
        print("Goal just went active")
        
    def feedback_callback(self, feedback):
        print(feedback.feedback)
    
    def done_callback(self, status, result):
        if status == actionlib.GoalStatus.SUCCEEDED:
            return_to_ui(ctx, f"发现垃圾，等待下一步指示。")
            print(f"Action succeeded! {result.result}")
        else:
            if result is not None:
                print(f"Action failed! {result.result}")
            
    def call(self, input_text):
        goal = NavStrGoal()
        goal.input = input_text  # 设置你想要发送的字符串
        self.client.send_goal(goal, done_cb=self.done_callback, active_cb=self.active_callback, feedback_cb=self.feedback_callback)
        self.client.wait_for_result()
        if self.client.get_state() == actionlib.GoalStatus.SUCCEEDED:
            result = self.client.get_result()
            print('Action Succeeded: %s', result.result)
        else:
            print('Action failed')
            
            
ctx = None
if __name__ == '__main__':
    socket_ip = os.getenv('NAV_SOCKET_IP', '127.0.0.1')
    socket_port = int(os.getenv('NAV_SOCKET_PORT', '10086'))
    thread_pool = []
    print(f"Server started, listening on {socket_ip}:{socket_port}")
    try:
        nav_client = SemanticNavClient()
        server = SocketServer(socket_ip, socket_port)
        server.start()
        print("Server started")
        while not rospy.is_shutdown():
            if server.accept_client():
                message = server.recv()
                if message is not None:
                    ctx = json.loads(message)
                    thread_pool.append(threading.Thread(target=nav_client.call, args=(ctx.get('input_text'),)))
                    thread_pool[-1].start()
                    message = None
            else:
                time.sleep(1)
    except Exception as e:
        print(f"sdsds: {e}")
    for t in thread_pool:
        t.join()
